
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       gp_matrix3.c
  * @author     baiyang
  * @date       2021-11-24
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "gp_matrix3.h"
/*-----------------------------------macro------------------------------------*/
#ifndef min
#define min(a, b) ((a) > (b) ? (b) : (a))
#endif

#ifndef equal
#define equal(a, b) (((a) - (b)) < (1e-15) && ((a) - (b)) > -(1e-15))
#endif
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void matrix3f_from_euler(matrix3f_t* mat, const float roll, const float pitch, const float yaw)
{
    const float c3 = cosf(pitch);
    const float s3 = sinf(pitch);
    const float s2 = sinf(roll);
    const float c2 = cosf(roll);
    const float s1 = sinf(yaw);
    const float c1 = cosf(yaw);

    mat->a.x = c1 * c3 - s1 * s2 * s3;
    mat->b.y = c1 * c2;
    mat->c.z = c3 * c2;
    mat->a.y = -c2*s1;
    mat->a.z = s3*c1 + c3*s2*s1;
    mat->b.x = c3*s1 + s3*s2*c1;
    mat->b.z = s1*s3 - s2*c1*c3;
    mat->c.x = -s3*c2;
    mat->c.y = s2;
}

void matrix3f_zeros(matrix3f_t* mat)
{
    vec3_zero(&mat->a);
    vec3_zero(&mat->b);
    vec3_zero(&mat->c);
}

void matrix3f_eye(matrix3f_t* mat)
{
    matrix3f_zeros(mat);

    mat->a.x = 1.0f;
    mat->b.y = 1.0f;
    mat->c.z = 1.0f;
}

/* dst = src1 + src2 */
void matrix3f_add(matrix3f_t* dst, const matrix3f_t* src1, const matrix3f_t* src2)
{
    vec3_add(&dst->a, &src1->a, &src2->a);
    vec3_add(&dst->b, &src1->b, &src2->b);
    vec3_add(&dst->c, &src1->c, &src2->c);
}

/* dst = src1 - src2 */
void matrix3f_sub(matrix3f_t* dst, const matrix3f_t* src1, const matrix3f_t* src2)
{
    vec3_sub(&dst->a, &src1->a, &src2->a);
    vec3_sub(&dst->b, &src1->b, &src2->b);
    vec3_sub(&dst->c, &src1->c, &src2->c);
}

/* dst = src1 * src2 */
// 不可自乘
void matrix3f_mul(matrix3f_t* dst, const matrix3f_t* src1, const matrix3f_t* src2)
{
    dst->a.x = src1->a.x * src2->a.x + src1->a.y * src2->b.x + src1->a.z * src2->c.x;
    dst->a.y = src1->a.x * src2->a.y + src1->a.y * src2->b.y + src1->a.z * src2->c.y;
    dst->a.z = src1->a.x * src2->a.z + src1->a.y * src2->b.z + src1->a.z * src2->c.z;

    dst->b.x = src1->b.x * src2->a.x + src1->b.y * src2->b.x + src1->b.z * src2->c.x;
    dst->b.y = src1->b.x * src2->a.y + src1->b.y * src2->b.y + src1->b.z * src2->c.y;
    dst->b.z = src1->b.x * src2->a.z + src1->b.y * src2->b.z + src1->b.z * src2->c.z;

    dst->c.x = src1->c.x * src2->a.x + src1->c.y * src2->b.x + src1->c.z * src2->c.x;
    dst->c.y = src1->c.x * src2->a.y + src1->c.y * src2->b.y + src1->c.z * src2->c.y;
    dst->c.z = src1->c.x * src2->a.z + src1->c.y * src2->b.z + src1->c.z * src2->c.z;
}

/* dst = m * v */
void matrix3f_mul_vec(Vector3f_t* dst, const matrix3f_t* m, const Vector3f_t* v)
{
    Vector3f_t vec = *v;

    dst->x = m->a.x * vec.x + m->a.y * vec.y + m->a.z * vec.z;
    dst->y = m->b.x * vec.x + m->b.y * vec.y + m->b.z * vec.z;
    dst->z = m->c.x * vec.x + m->c.y * vec.y + m->c.z * vec.z;
}

/* dst = src1 * src2 */
void matrix3f_mul_transpose_vec(Vector3f_t* dst, const matrix3f_t* m, const Vector3f_t* v)
{
    Vector3f_t vec = *v;

    dst->x = m->a.x * vec.x + m->b.x * vec.y + m->c.x * vec.z;
    dst->y = m->a.y * vec.x + m->b.y * vec.y + m->c.y * vec.z;
    dst->z = m->a.z * vec.x + m->b.z * vec.y + m->c.z * vec.z;
}

/* dst = src' */
void matrix3f_trans(matrix3f_t* dst, const matrix3f_t* src)
{
    dst->a.x = src->a.x;
    dst->a.y = src->b.x;
    dst->a.z = src->c.x;

    dst->b.x = src->a.y;
    dst->b.y = src->b.y;
    dst->b.z = src->c.y;

    dst->c.x = src->a.z;
    dst->c.y = src->b.z;
    dst->c.z = src->c.z;
}
/*------------------------------------test------------------------------------*/


